/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include "LOS_EPSCtrl.h"

uint8_t EPS_RCV_FLAG = 0;

LOS_EPS_t STA_EPS;

uint8_t PDSW_1_8 = 0;
uint8_t PDSW_9_16 = 0;
uint8_t PDSW_17_24 = 0;

uint8_t PDSW_1_8_cp1 = 0;
uint8_t PDSW_9_16_cp1 = 0;
uint8_t PDSW_17_24_cp1 = 0;

uint8_t PDSW_1_8_cp2 = 0;
uint8_t PDSW_9_16_cp2 = 0;
uint8_t PDSW_17_24_cp2 = 0;

uint8_t TCSW_1_8 = 0;
uint8_t TCSW_9_16 = 0;
uint8_t TCSW_17_24 = 0;

uint8_t TCSW_1_8_cp1 = 0;
uint8_t TCSW_9_16_cp1 = 0;
uint8_t TCSW_17_24_cp1 = 0;

uint8_t TCSW_1_8_cp2 = 0;
uint8_t TCSW_9_16_cp2 = 0;
uint8_t TCSW_17_24_cp2 = 0;

uint8_t TC_TEMPNUM = 0XFF;
uint8_t TC_TEMPUP = 0XFF;
uint8_t TC_TEMPLOW = 0XFF;

uint8_t TCPoint = 0xFF;
int8_t TCTemp = -1;

uint8_t TMPACK_PDB[138];
uint8_t TMPACK_TCB[138];

uint16_t PD_TCCNT = 0;
uint16_t TC_TCCNT = 0;

/** @fn void LOS_EPS_GetEXSTA()
*   @brief 配电扩展板状态遥测请求
*/
void LOS_EPS_GetEXSTA()
{
    uint8_t CMD[9] = {0xEB, 0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0X00};
    uint32_t i = 0;
    CMD[2] = LOS_EPS_TWOTHREE(PDSW_1_8, PDSW_1_8_cp1, PDSW_1_8_cp2);
    CMD[3] = LOS_EPS_TWOTHREE(PDSW_9_16, PDSW_9_16_cp1, PDSW_9_16_cp2);
    CMD[4] = LOS_EPS_TWOTHREE(PDSW_17_24, PDSW_17_24_cp1, PDSW_17_24_cp2);
    CMD[8] = LOS_FP_CRC8Cal(CMD, 8);
    STA_EPS.HL_TXF_EX = 1;
    LOS_MAX14830_ResetFIFO(EPSCTRL_MAXNUM, EPSCTRL_MAXPort);
    LOS_MAX14830_WriteBytes(EPSCTRL_MAXNUM, EPSCTRL_MAXPort, CMD ,9);
    PD_TCCNT++;
}


/** @fn void LOS_EPS_Switch(uint8_t payload, uint8_t sta)
*   @brief 控制配电扩展板开关
*   @param[in] payload 开关号，通过宏定义与具体单机绑定
*   @param[in] sta 开关状态 SWITCH_ON和SWITCH_OFF
*/
void LOS_EPS_Switch(uint8_t payload, uint8_t sta)
{
    if (sta == SWITCH_ON) //开启
    {
        if ((payload >= 1) && (payload <= 8))
        {
            PDSW_1_8 |= (1 << (payload - 1));
            PDSW_1_8_cp1 |= (1 << (payload - 1)); 
            PDSW_1_8_cp2 |= (1 << (payload - 1)); 
        }
        else if ((payload >= 9) && (payload <= 16))
        {
            PDSW_9_16 |= (1 << (payload - 8 - 1));
            PDSW_9_16_cp1 |= (1 << (payload - 8 - 1));
            PDSW_9_16_cp2 |= (1 << (payload - 8 - 1));
        }
        else if ((payload >= 17) && (payload <= 24))
        {
            PDSW_17_24 |= (1 << (payload - 16 - 1));
            PDSW_17_24_cp1 |= (1 << (payload - 16 - 1));
            PDSW_17_24_cp2 |= (1 << (payload - 16 - 1));
            
        }
    }
    else if (sta == SWITCH_OFF) //关闭
    {
        if ((payload >= 1) && (payload <= 8))
        {
            PDSW_1_8 &= ~(1 << (payload - 1));
            PDSW_1_8_cp1 &= ~(1 << (payload - 1));
            PDSW_1_8_cp2 &= ~(1 << (payload - 1));
        }
        else if ((payload >= 9) && (payload <= 16))
        {
            PDSW_9_16 &= ~(1 << (payload - 8 - 1));
            PDSW_9_16_cp1 &= ~(1 << (payload - 8 - 1));
            PDSW_9_16_cp2 &= ~(1 << (payload - 8 - 1));
        }
        else if ((payload >= 17) && (payload <= 24))
        {
            PDSW_17_24 &= ~(1 << (payload - 16 - 1));
            PDSW_17_24_cp1 &= ~(1 << (payload - 16 - 1));
            PDSW_17_24_cp2 &= ~(1 << (payload - 16 - 1));
        }
    }
    LOS_PCDU_CMDCNT++;
}

void LOS_EPS_PDTKSW(uint8_t payload, uint32_t dur)
{
    static uint8_t Ctrl[24] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    if (dur == 0)
    {
        LOS_EPS_Switch(payload, SWITCH_ON); //保持开启，占空比100%
    }
    else
    {
        if (Ctrl[payload] == 0)
        {
            LOS_EPS_Switch(payload, SWITCH_ON);
            Ctrl[payload]++;
        }
        else if (Ctrl[payload] == 1)
        {
            LOS_EPS_Switch(payload, SWITCH_OFF);
            if (dur >  1)
            {
                Ctrl[payload]++;
            }
            else
            {
                Ctrl[payload] = 0;
            }
        }
        else if (Ctrl[payload] >= dur)
        {
            LOS_EPS_Switch(payload, SWITCH_OFF);
            Ctrl[payload] = 0;
        }
        else 
        {
            LOS_EPS_Switch(payload, SWITCH_OFF);
            Ctrl[payload]++;
        }
    }
}

void LOS_EPS_TCTKSW(uint8_t payload, uint32_t dur)
{
    static uint8_t Ctrl[24] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
    if (dur == 0)
    {
        LOS_EPS_TCSwitch(payload, SWITCH_ON); //保持开启，占空比100%
    }
    else
    {
        if (Ctrl[payload] == 0)
        {
            LOS_EPS_TCSwitch(payload, SWITCH_ON);
            Ctrl[payload]++;
        }
        else if (Ctrl[payload] == 1)
        {
            LOS_EPS_TCSwitch(payload, SWITCH_OFF);
            if (dur >  1)
            {
                Ctrl[payload]++;
            }
            else
            {
                Ctrl[payload] = 0;
            }
        }
        else if (Ctrl[payload] >= dur)
        {
            LOS_EPS_TCSwitch(payload, SWITCH_OFF);
            Ctrl[payload] = 0;
        }
        else 
        {
            LOS_EPS_TCSwitch(payload, SWITCH_OFF);
            Ctrl[payload]++;
        }
    }
}

/** @fn void LOS_EPS_ResetPD()
*   @brief 软件重启配电扩展板
*/
void LOS_EPS_ResetPD()
{
    uint8_t CMD[8] = {0xEB, 0x11, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0XFF};
    uint32_t i = 0;
    LOS_MAX14830_ResetFIFO(EPSCTRL_MAXNUM, EPSCTRL_MAXPort);
    LOS_MAX14830_WriteBytes(EPSCTRL_MAXNUM, EPSCTRL_MAXPort, CMD ,8);
    STA_EPS.RBCNT[0]++;
}


/** @fn void LOS_EPS_ResetPDH()
*   @brief 硬件重启配电扩展板
*   @note 需要配合LOS_EPS_RecoverPDH()使用
*/
void LOS_EPS_ResetPDH()
{
    if (STA_EPS.PD_RSTFLAG == 0)
    {
        gioSetBit(mibspiPORT1, PIN_CS0, 1);
        STA_EPS.PD_RSTFLAG = 1;
    }
}

/** @fn void LOS_EPS_ResetPDH()
*   @brief 硬件恢复配电扩展板
*   @note 拉高复位引脚后，应在线程中调用该函数将引脚重新拉低，完成硬件复位
*/
void LOS_EPS_RecoverPDH()
{
    gioSetBit(mibspiPORT1, PIN_CS0, 0);
    STA_EPS.RBCNT[0]++;
}


/** @fn void LOS_EPS_ResetEPS()
*   @brief 硬件重启电源板
*/
void LOS_EPS_ResetEPS()
{
    uint8_t CMD[8] = {0xEB, 0x12, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
    uint32_t i = 0;
    CMD[7] = CMD[0] + CMD[1] + CMD[2] + CMD[3] + CMD[4] + CMD[5] + CMD[6];
    LOS_MAX14830_ResetFIFO(EPSCTRL_MAXNUM, EPSCTRL_MAXPort);
    LOS_MAX14830_WriteBytes(EPSCTRL_MAXNUM, EPSCTRL_MAXPort, CMD ,8);
    STA_EPS.RBCNT[2]++;
}


/** @fn void LOS_EPS_GetTCSTA()
*   @brief 热控扩展板状态遥测请求
*/
void LOS_EPS_GetTCSTA()
{
    uint8_t CMD[9] = {0xEB, 0x21, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,0x00};
    uint32_t i = 0;
    CMD[2] = LOS_EPS_TWOTHREE(TCSW_1_8, TCSW_1_8_cp1, TCSW_1_8_cp2);
    CMD[3] = LOS_EPS_TWOTHREE(TCSW_9_16, TCSW_9_16_cp1, TCSW_9_16_cp2);
    CMD[4] = LOS_EPS_TWOTHREE(TCSW_17_24, TCSW_17_24_cp1, TCSW_17_24_cp2);
    CMD[5] = TC_TEMPNUM;
    CMD[6] = TC_TEMPUP;
    CMD[7] = TC_TEMPLOW;
    CMD[8] = LOS_FP_CRC8Cal(CMD, 8);
    STA_EPS.HL_TXF_TC = 1;
    LOS_MAX14830_ResetFIFO(TEST_MAXNUM, TEST_MAXPort);
    LOS_MAX14830_WriteBytes(TEST_MAXNUM, TEST_MAXPort, CMD ,9);
    TC_TCCNT++;
}


/** @fn void LOS_EPS_TCSwitch(uint8_t payload, uint8_t sta)
*   @brief 控制热控扩展板开关
*   @param[in] payload 开关号，通过宏定义与具体单机绑定
*   @param[in] sta 开关状态 SWITCH_ON和SWITCH_OFF
*/
void LOS_EPS_TCSwitch(uint8_t payload, uint8_t sta)
{
    if (sta == SWITCH_ON) //开启
    {
        if ((payload >= 1) && (payload <= 8))
        {
            TCSW_1_8 |= (1 << (payload - 1));
            TCSW_1_8_cp1 |= (1 << (payload - 1));
            TCSW_1_8_cp2 |= (1 << (payload - 1));
        }
        else if ((payload >= 9) && (payload <= 16))
        {
            TCSW_9_16 |= (1 << (payload - 8 - 1));
            TCSW_9_16_cp1 |= (1 << (payload - 8 - 1));
            TCSW_9_16_cp2 |= (1 << (payload - 8 - 1));
        }
        else if ((payload >= 17) && (payload <= 24))
        {
            TCSW_17_24 |= (1 << (payload - 16 - 1));
            TCSW_17_24_cp1 |= (1 << (payload - 16 - 1));
            TCSW_17_24_cp2 |= (1 << (payload - 16 - 1));
        }
    }
    else if (sta == SWITCH_OFF) //关闭
    {
        if ((payload >= 1) && (payload <= 8))
        {
            TCSW_1_8 &= ~(1 << (payload - 1));
            TCSW_1_8_cp1 &= ~(1 << (payload - 1));
            TCSW_1_8_cp2 &= ~(1 << (payload - 1));
        }
        else if ((payload >= 9) && (payload <= 16))
        {
            TCSW_9_16 &= ~(1 << (payload - 8 - 1));
            TCSW_9_16_cp1 &= ~(1 << (payload - 8 - 1));
            TCSW_9_16_cp2 &= ~(1 << (payload - 8 - 1));
        }
        else if ((payload >= 17) && (payload <= 24))
        {
            TCSW_17_24 &= ~(1 << (payload - 16 - 1));
            TCSW_17_24_cp1 &= ~(1 << (payload - 16 - 1));
            TCSW_17_24_cp2 &= ~(1 << (payload - 16 - 1));
        }
    }
}

/** @fn void LOS_EPS_TurnOffAll()
*   @brief 关闭所有开关
*/
void LOS_EPS_TurnOffAll()
{
    PDSW_1_8 = 0;
    PDSW_1_8_cp1 = 0;
    PDSW_1_8_cp2 = 0;
    PDSW_9_16 = 0;
    PDSW_9_16_cp1 = 0;
    PDSW_9_16_cp2 = 0;
    PDSW_17_24 = 0;
    PDSW_17_24_cp1 = 0;
    PDSW_17_24_cp2 = 0;
    TCSW_1_8 = 0;
    TCSW_1_8_cp1 = 0;
    TCSW_1_8_cp2 = 0;
    TCSW_9_16 = 0;
    TCSW_9_16_cp1 = 0;
    TCSW_9_16_cp2 = 0;
    TCSW_17_24 = 0;
    TCSW_17_24_cp1 = 0;
    TCSW_17_24_cp2 = 0;
}


/** @fn void LOS_EPS_ResetTC()
*   @brief 软件重启热控扩展板
*/
void LOS_EPS_ResetTC()
{
    uint8_t CMD[7] = {0xEB, 0x32, 0x0D, 0xFF, 0xFF, 0xFF, 0xFF};
    uint32_t i = 0;
    gioSetBit(gioPORTB, 4, 1);
    LOS_MAX14830_ResetFIFO(EPSCTRL_MAXNUM, EPSCTRL_MAXPort);
    CMD[2] = CMD[0] + CMD[1];
    LOS_MAX14830_WriteBytes(EPSCTRL_MAXNUM, EPSCTRL_MAXPort, CMD ,7);
    for (i=0;i<25000;i++);
    gioSetBit(gioPORTB, 4, 0);
    STA_EPS.RBCNT[1]++;
}

/** @fn void LOS_EPS_ResetTCH()
*   @brief 硬件重启热控扩展板
*   @note 需要配合LOS_EPS_ResetTCH()使用
*/
void LOS_EPS_ResetTCH()
{   if (STA_EPS.TC_RSTFLAG == 0)
    {
        gioSetBit(mibspiPORT1, PIN_CS1, 1);
        STA_EPS.TC_RSTFLAG = 1;
    }
}

/** @fn void LOS_EPS_ResetPDH()
*   @brief 硬件恢复热控扩展板
*   @note 拉高复位引脚后，应在线程中调用该函数将引脚重新拉低，完成硬件复位
*/
void LOS_EPS_RecoverTCH()
{
    gioSetBit(mibspiPORT1, PIN_CS1, 0);
    STA_EPS.RBCNT[1]++;
}


/** @fn void LOS_EPS_TCSetTemp(uint8_t position, int8_t temp)
*   @brief 设置加热温度
*   @param[in] position 测温点
*   @param[in] tempup 加热温度上限
*   @param[in] templow 加热温度下限
*/
void LOS_EPS_TCSetTemp(uint8_t position, uint8_t tempup, uint8_t templow)
{
    TC_TEMPNUM = position;
    TC_TEMPUP = tempup;
    TC_TEMPLOW = templow;
}

/** @fn void LOS_EPS_GetSTA()
*   @brief 配电扩展板与热控扩展板遥测请求
*/
void LOS_EPS_GetSTA()
{
    static uint8_t control = 0;
    static uint32_t LOS_TK_TimeCtrl[8] = {0, 0, 0, 0, 0, 0, 0, 0}; //持续时间控制
    static uint32_t LOS_TK_CNTCtrl[8] = {0, 0, 0, 0, 0, 0, 0, 0};  //最大次数控制

    if (control % 2 == 0)  //配电扩展板，500ms轮询一次
    {
        //+X太阳翼热刀
        if (LOS_TK_SEL[TKSAX1_CODE] == 0xAA)  
        {
            if (LOS_TK_CNTCtrl[TKSAX1_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_PDTKSW(SWITCH_SAX1_PD, FR_TKDUR);
                LOS_TK_CNTCtrl[TKSAX1_CODE]++;
            }
            else 
            {
                LOS_EPS_Switch(SWITCH_SAX1_PD, SWITCH_OFF);
                LOS_TK_SEL[TKSAX1_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKSAX1_CODE] = 0;
                LOS_TK_TimeCtrl[TKSAX1_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_Switch(SWITCH_SAX1_PD, SWITCH_OFF);
        }

        //-X太阳翼热刀
        if (LOS_TK_SEL[TKSAX2_CODE] == 0xAA)
        {
            if (LOS_TK_CNTCtrl[TKSAX2_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_PDTKSW(SWITCH_SAX2_PD, FR_TKDUR);
                LOS_TK_CNTCtrl[TKSAX2_CODE]++;
            }
            else
            {
                LOS_EPS_Switch(SWITCH_SAX2_PD, SWITCH_OFF);
                LOS_TK_SEL[TKSAX2_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKSAX2_CODE] = 0;
                LOS_TK_TimeCtrl[TKSAX2_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_Switch(SWITCH_SAX2_PD, SWITCH_OFF);
        }

        //遮光罩热刀1
        if (LOS_TK_SEL[TKLEN1_CODE] == 0XAA)
        {
            if (LOS_TK_CNTCtrl[TKLEN1_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_PDTKSW(SWITCH_LEN1_PD, FR_TKDUR);
                LOS_TK_CNTCtrl[TKLEN1_CODE]++;
            }
            else
            {
                LOS_EPS_Switch(SWITCH_LEN1_PD, SWITCH_OFF);
                LOS_TK_SEL[TKLEN1_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKLEN1_CODE] = 0;
                LOS_TK_TimeCtrl[TKLEN1_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_Switch(SWITCH_LEN1_PD, SWITCH_OFF);
        }

        //遮光罩热刀2
        if (LOS_TK_SEL[TKLEN2_CODE] == 0XAA)
        {
            if (LOS_TK_CNTCtrl[TKLEN2_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_PDTKSW(SWITCH_LEN2_PD, FR_TKDUR);
                LOS_TK_CNTCtrl[TKLEN2_CODE]++;
            }
            else
            {
                LOS_EPS_Switch(SWITCH_LEN2_PD, SWITCH_OFF);
                LOS_TK_SEL[TKLEN2_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKLEN2_CODE] = 0;
                LOS_TK_TimeCtrl[TKLEN2_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_Switch(SWITCH_LEN2_PD, SWITCH_OFF);
        }

        taskENTER_CRITICAL();
        LOS_EPS_GetEXSTA();
        taskEXIT_CRITICAL();
        control++;
    }
    else  //热控扩展板，500ms轮询一次
    {
        //UHF1热刀
        if (LOS_TK_SEL[TKUHF1_CODE] == 0XAA)
        {
            if (LOS_TK_CNTCtrl[TKUHF1_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_TCTKSW(SWITCH_UHF1_TC, FR_TKDUR);
                LOS_TK_CNTCtrl[TKUHF1_CODE]++;
            }
            else
            {
                LOS_EPS_TCSwitch(SWITCH_UHF1_TC, SWITCH_OFF);
                LOS_TK_SEL[TKUHF1_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKUHF1_CODE] = 0;
                LOS_TK_TimeCtrl[TKUHF1_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_TCSwitch(SWITCH_UHF1_TC, SWITCH_OFF);
        }

        //UHF2热刀
        if (LOS_TK_SEL[TKUHF2_CODE] == 0XAA)
        {
            if (LOS_TK_CNTCtrl[TKUHF2_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_TCTKSW(SWITCH_UHF2_TC, FR_TKDUR);
                LOS_TK_CNTCtrl[TKUHF2_CODE]++;
            }
            else
            {
                LOS_EPS_TCSwitch(SWITCH_UHF2_TC, SWITCH_OFF);
                LOS_TK_SEL[TKUHF2_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKUHF2_CODE] = 0;
                LOS_TK_TimeCtrl[TKUHF2_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_TCSwitch(SWITCH_UHF2_TC, SWITCH_OFF);
        }

        //VHF1热刀
        if (LOS_TK_SEL[TKVHF1_CODE] == 0XAA)
        {
            if (LOS_TK_CNTCtrl[TKVHF1_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_TCTKSW(SWITCH_VHF1_TC, FR_TKDUR);
                LOS_TK_CNTCtrl[TKVHF1_CODE]++;
            }
            else
            {
                LOS_EPS_TCSwitch(SWITCH_VHF1_TC, SWITCH_OFF);
                LOS_TK_SEL[TKVHF1_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKVHF1_CODE] = 0;
                LOS_TK_TimeCtrl[TKVHF1_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_TCSwitch(SWITCH_VHF1_TC, SWITCH_OFF);
        }

        //VHF2热刀
        if (LOS_TK_SEL[TKVHF2_CODE] == 0XAA)
        {
            if (LOS_TK_CNTCtrl[TKVHF2_CODE] < FR_TKMAXCNT*(FR_TKDUR + 1))
            {
                LOS_EPS_TCTKSW(SWITCH_VHF2_TC, FR_TKDUR);
                LOS_TK_CNTCtrl[TKVHF2_CODE]++;
            }
            else
            {
                LOS_EPS_TCSwitch(SWITCH_VHF2_TC, SWITCH_OFF);
                LOS_TK_SEL[TKVHF2_CODE] = 0x55;
                LOS_TK_CNTCtrl[TKVHF2_CODE] = 0;
                LOS_TK_TimeCtrl[TKVHF2_CODE] = 0;
            }
        }
        else
        {
            LOS_EPS_TCSwitch(SWITCH_VHF2_TC, SWITCH_OFF);
        }


        taskENTER_CRITICAL();
        LOS_EPS_GetTCSTA();
        taskEXIT_CRITICAL();
        control++;
    }
}

/** @fn uint8_t LOS_EPS_TWOTHREE(uint8_t cp0, uint8_t cp1, uint8_t cp2)
*   @brief 开关量三取二函数
*   @param[in] cp0 开关量
*   @param[in] cp1 开关量副本1
*   @param[in] cp2 开关量副本2
*/
uint8_t LOS_EPS_TWOTHREE(uint8_t cp0, uint8_t cp1, uint8_t cp2)
{
    if (cp0 != cp1)
    {
        if (cp0 == cp2)
        {
            return cp0;
        }
        else if (cp1 == cp2)
        {
            return cp1;
        }
        else
        {
            return cp0;
        }
    }
    else if (cp0 != cp2)
    {
        if (cp0 == cp1)
        {
            return cp0;
        }
        else if (cp1 == cp2)
        {
            return cp2;
        }
        else
        {
            return cp0;
        }
    }
    else if (cp1 != cp2)
    {
        if (cp1 == cp0)
        {
            return cp1;
        }
        else if (cp0 == cp2)
        {
            return cp2;
        }
        else
        {
            return cp0;
        }
    }
    else
    {
        return cp0;
    }
}

/** @fn uint8_t LOS_EPS_DataProcess(uint8_t* data)
*   @brief 电源遥测信息处理
*   @param[in] data 遥测数据包
*/
EPS_ENUM LOS_EPS_DataProcess(uint8_t* data)
{
    uint8_t checksum = 0;
    uint8_t i = 0;
    uint8_t crc = 0;
    float buf;
    static uint32_t lasttime = 0;  //上一次收到电源遥测的累计秒，用来算安时计的，只在安全模式下有用
    static uint32_t safecnt = 0;   //安全模式计数，记录在安全模式下安时计计算了多少次
    float char_cur = 0; //充电器1电流，用来算安时计的
    
    if (data[0]!=0x14)    //判断包首
    {
        return EPS_WrongHead;
    }
    else
    {
        if (data[1]==0x01)  //判断包类型
        {

            memcpy(&TMPACK_PDB[0], &LOS_Time_SecFromBoot, 4);
            memcpy(&TMPACK_PDB[4], &LOS_Time_MilSec, 2);
            memcpy(&TMPACK_PDB[6], &LOS_Time_Sec, 4);
            memcpy(&TMPACK_PDB[10], &data[2], 107);
            

            crc = LOS_FP_CRC8Cal(data,126);
            if  (crc != data[126])
            {
                STA_EPS.PD_CRCERRCNT++;
            }
            // if (crc == data[127])
            // {
                STA_EPS.PD_LastTime = LOS_Time_SecFromBoot; //记录收到这一包的时间
                STA_EPS.TMCNT_PD++;
                //开关状态原始数据
                STA_EPS.SWITCH_PD_O[0] = data[2];
                STA_EPS.SWITCH_PD_O[1] = data[3];
                STA_EPS.SWITCH_PD_O[2] = data[4];
                //开关状态处理（别抠门，不缺这点内存）
                STA_EPS.SWITCH_PD[0]=(data[2] & (1 << 0))>>0;
                STA_EPS.SWITCH_PD[1]=(data[2] & (1 << 1))>>1;
                STA_EPS.SWITCH_PD[2]=(data[2] & (1 << 2))>>2;
                STA_EPS.SWITCH_PD[3]=(data[2] & (1 << 3))>>3;
                STA_EPS.SWITCH_PD[4]=(data[2] & (1 << 4))>>4;
                STA_EPS.SWITCH_PD[5]=(data[2] & (1 << 5))>>5;
                STA_EPS.SWITCH_PD[6]=(data[2] & (1 << 6))>>6;
                STA_EPS.SWITCH_PD[7]=(data[2] & (1 << 7))>>7;
                STA_EPS.SWITCH_PD[8]=(data[3] & (1 << 0))>>0;
                STA_EPS.SWITCH_PD[9]=(data[3] & (1 << 1))>>1;
                STA_EPS.SWITCH_PD[10]=(data[3] & (1 << 2))>>2;
                STA_EPS.SWITCH_PD[11]=(data[3] & (1 << 3))>>3;
                STA_EPS.SWITCH_PD[12]=(data[3] & (1 << 4))>>4;
                STA_EPS.SWITCH_PD[13]=(data[3] & (1 << 5))>>5;
                STA_EPS.SWITCH_PD[14]=(data[3] & (1 << 6))>>6;
                STA_EPS.SWITCH_PD[15]=(data[3] & (1 << 7))>>7;
                STA_EPS.SWITCH_PD[16]=(data[4] & (1 << 0))>>0;
                STA_EPS.SWITCH_PD[17]=(data[4] & (1 << 1))>>1;
                STA_EPS.SWITCH_PD[18]=(data[4] & (1 << 2))>>2;
                STA_EPS.SWITCH_PD[19]=(data[4] & (1 << 3))>>3;
                STA_EPS.SWITCH_PD[20]=(data[4] & (1 << 4))>>4;
                STA_EPS.SWITCH_PD[21]=(data[4] & (1 << 5))>>5;
                STA_EPS.SWITCH_PD[22]=(data[4] & (1 << 6))>>6;
                STA_EPS.SWITCH_PD[23]=(data[4] & (1 << 7))>>7;
                for(i=0; i<24; i++)
                {
                    if (STA_EPS.SWITCH_PD[i] == 1)
                    {
                        STA_EPS.SWITCH_PD[i] = 0xAA;
                    }
                    else
                    {
                        STA_EPS.SWITCH_PD[i] = 0x55;
                    }
                }

                memcpy(&(STA_EPS.Cur_PD25940[0]),&data[5],48); //24路电流值
                STA_EPS.Cur_PDMSP430 = data[53];               //MSP430电流值
                STA_EPS.ball = data[54];
                //温度遥测
                memcpy(STA_EPS.Temp_MPPT, &data[55], 3);
                memcpy(STA_EPS.Temp_Charger, &data[58], 2);
                memcpy(&STA_EPS.Temp_3V3, &data[60], 1);
                memcpy(&STA_EPS.Temp_5V, &data[61], 1);
                memcpy(&STA_EPS.Temp_12V, &data[62], 1);
                //电压遥测
                memcpy(STA_EPS.Vol_MPPT, &data[63], 6);
                memcpy(STA_EPS.Vol_Charger, &data[69], 4);
                memcpy(&STA_EPS.Vol_3V3, &data[73], 2);
                memcpy(&STA_EPS.Vol_5V, &data[75], 2);
                memcpy(&STA_EPS.Vol_12V, &data[77], 2);
                //电流遥测
                memcpy(STA_EPS.Cur_MPPT, &data[79], 6);
                memcpy(STA_EPS.Cur_Charger, &data[85], 4);
                char_cur = STA_EPS.Cur_Charger[0]/1000; //计算安时计用，单位mA转A
                memcpy(&STA_EPS.Cur_3V3, &data[89], 2);
                memcpy(&STA_EPS.Cur_5V, &data[91], 2);
                memcpy(&STA_EPS.Cur_12V, &data[93], 2);
                //帆板遥测
                memcpy(STA_EPS.Vol_SA, &data[95], 6);
                //遥测计数
                memcpy(&STA_EPS.PD_CMDCNT, &data[101], 2);
                //错误指令计数
                memcpy(&STA_EPS.PD_CMDERRCNT, &data[103], 2);
                //最后一条指令
                memcpy(&STA_EPS.PD_LASTCMD, &data[105], 2);

                /* 安时计计算 */
//                if (MODE_AOCS.mode == Safe) //如果是安全模式
//                {
//                    if (lasttime == 0) //第一次进入
//                    {
//                        STA_EPS.AhMeter = 0; //安时计先归一次零
//                        lasttime = LOS_Time_SecFromBoot;
//                    }
//                    else
//                    {
//                        STA_EPS.AhMeter += (LOS_Time_SecFromBoot - lasttime) * char_cur / 3600; //单位s转h
//                        lasttime = LOS_Time_SecFromBoot;
//                    }
//                }
//                else
//                {
//                    STA_EPS.AhMeter = 0;
//                    lasttime = 0;
//                    safecnt = 0;
//                }

                /* 母线电压计算 */
                if (STA_EPS.Vol_Charger[0] > STA_EPS.Vol_Charger[1]) //太阳翼没有没有受到照射
                {
                    STA_EPS.Mother_Vol = STA_EPS.Vol_Charger[0];
                }
                else 
                {
                    STA_EPS.Mother_Vol = STA_EPS.Vol_Charger[1];
                }

            // }
            // else
            // {
            //     STA_EPS.PD_CRCERRCNT++;
            // }
            
        }
        else if (data[1] == 0x21)
        {
            memcpy(&TMPACK_TCB[0], &LOS_Time_SecFromBoot, 4);
            memcpy(&TMPACK_TCB[4], &LOS_Time_MilSec, 2);
            memcpy(&TMPACK_TCB[6], &LOS_Time_Sec, 4);
            memcpy(&TMPACK_TCB[10], &data[2], 123);

            STA_EPS.TC_LastTime = LOS_Time_SecFromBoot; //记录收到这一包的时间
            crc = LOS_FP_CRC8Cal(data,126);
            if  (crc != data[126])
            {
                STA_EPS.TC_CRCERRCNT++;
            }
            // if (crc == data[127])
            // {
                STA_EPS.TMCNT_TC++;
                //开关状态原始数据
                STA_EPS.SWITCH_TC_O[0] = data[2];
                STA_EPS.SWITCH_TC_O[1] = data[3];
                STA_EPS.SWITCH_TC_O[2] = data[4];
                //开关状态处理（别抠门，不缺这点内存）
                STA_EPS.SWITCH_TC[0]=(data[2] & (1 << 0))>>0;
                STA_EPS.SWITCH_TC[1]=(data[2] & (1 << 1))>>1;
                STA_EPS.SWITCH_TC[2]=(data[2] & (1 << 2))>>2;
                STA_EPS.SWITCH_TC[3]=(data[2] & (1 << 3))>>3;
                STA_EPS.SWITCH_TC[4]=(data[2] & (1 << 4))>>4;
                STA_EPS.SWITCH_TC[5]=(data[2] & (1 << 5))>>5;
                STA_EPS.SWITCH_TC[6]=(data[2] & (1 << 6))>>6;
                STA_EPS.SWITCH_TC[7]=(data[2] & (1 << 7))>>7;
                STA_EPS.SWITCH_TC[8]=(data[3] & (1 << 0))>>0;
                STA_EPS.SWITCH_TC[9]=(data[3] & (1 << 1))>>1;
                STA_EPS.SWITCH_TC[10]=(data[3] & (1 << 2))>>2;
                STA_EPS.SWITCH_TC[11]=(data[3] & (1 << 3))>>3;
                STA_EPS.SWITCH_TC[12]=(data[3] & (1 << 4))>>4;
                STA_EPS.SWITCH_TC[13]=(data[3] & (1 << 5))>>5;
                STA_EPS.SWITCH_TC[14]=(data[3] & (1 << 6))>>6;
                STA_EPS.SWITCH_TC[15]=(data[3] & (1 << 7))>>7;
                STA_EPS.SWITCH_TC[16]=(data[4] & (1 << 0))>>0;
                STA_EPS.SWITCH_TC[17]=(data[4] & (1 << 1))>>1;
                STA_EPS.SWITCH_TC[18]=(data[4] & (1 << 2))>>2;
                STA_EPS.SWITCH_TC[19]=(data[4] & (1 << 3))>>3;
                STA_EPS.SWITCH_TC[20]=(data[4] & (1 << 4))>>4;
                STA_EPS.SWITCH_TC[21]=(data[4] & (1 << 5))>>5;
                STA_EPS.SWITCH_TC[22]=(data[4] & (1 << 6))>>6;
                STA_EPS.SWITCH_TC[23]=(data[4] & (1 << 7))>>7;
                for(i=0; i<24; i++)
                {
                    if (STA_EPS.SWITCH_TC[i] == 1)
                    {
                        STA_EPS.SWITCH_TC[i] = 0xAA;
                    }
                    else
                    {
                        STA_EPS.SWITCH_TC[i] = 0x55;
                    }
                }

                memcpy(&(STA_EPS.Cur_TC25940[0]),&data[5],48); //24路电流值
                STA_EPS.Cur_TCMSP430 = data[53];               //MSP430电流值

                //加热带温度上限
                memcpy(STA_EPS.Heater_UP, &data[54], 14);
                //加热带温度下限
                memcpy(STA_EPS.Heater_LOW, &data[68], 14);
                //热敏电阻温度值
                memcpy(STA_EPS.Temp_Ther, &data[82], 32);

                //遥测计数
                memcpy(&STA_EPS.TC_CMDCNT, &data[114], 2);
                //错误指令计数
                memcpy(&STA_EPS.TC_CMDERRCNT, &data[116], 2);
            // }
            // else
            // {
            //     STA_EPS.TC_CRCERRCNT++;
            // }

            
        }
        return EPS_TMOK;
    }
}

